/*
 * pid.h
 *
 *  Created on: 2021年10月29日
 *  Author : wuhexing
 */
#include "pid.h"

void PositionPidRealize(PID* vPID);
void IncrementPidRealize(PID* vPID);
void ExpertPidRealize(PID* vPID);

/*static inline float myfabs(float data)
{
	return data >= 0 ? data:(-data);
}*/
static float myfabs(float data)
{
	return data >= 0 ? data:(-data);
}

void Pid_Init(PID* vPID)
{
	vPID->setPoint = 0.00f;
	vPID->actualPoint = 0.00f;
	vPID->preError = 0.00f;
	vPID->pre2Error = 0.00f;
	vPID->integral = 0.00f;
    vPID->integral_max = 0.00f;
    vPID->KP = 5.0f;
	vPID->KI = 0.01f;
	vPID->KD = 0.0f;
	vPID->result = 0.0f;
	vPID->maximum = 500;
	vPID->minimum = -500;
	vPID->index = 0;
	vPID->integral_fenli = 500;

	vPID->errorabsmax = 150;
	vPID->errorabsmid = 50.0;
	vPID->errorabsmin = 1.0;
}

void Pid_Realize(PID* vPID, int PIDREALIZE_SELSECT)
{
	switch(PIDREALIZE_SELSECT)
	{
	case POSITION_PID:
		PositionPidRealize(vPID);
		break;
	case INCREATEMENT_PID:
		IncrementPidRealize(vPID);
		break;
	case EXPERT_PID:
		ExpertPidRealize(vPID);
		break;
	}
}

//函数名称：位置型PID实现
void PositionPidRealize(PID* vPID)
{
	float err;
	float result;
	err = vPID->setPoint - vPID->actualPoint;
	vPID->integral += err;
	/* 积分限幅 */
    if(vPID->integral > vPID->integral_max){
        vPID->integral = vPID->integral_max;
    }
	
	/* 积分分离 */
	if(vPID->preError >= integral_fenli){
		vPID->index = 0;
	}else{
		vPID->index = 1;
	}

	result = vPID->KP * err + vPID->KI * vPID->integral * vPID->index + vPID->KD * (err - vPID->preError);
	vPID->preError = err;

    /* 输出限幅 */
    if(result > vPID->maximum)
        vPID->result = vPID->maximum;
    else if(result < vPID->minimum)
        vPID->result = vPID->minimum;
    else
        vPID->result = result;
}

//函数名称：增量型PID实现
void IncrementPidRealize(PID* vPID)
{
	float err,increment;
	err = vPID->setPoint - vPID->actualPoint;
	increment = vPID->KP * (err - vPID->preError) + vPID->KI * err + vPID->KD * (err - 2 * vPID->preError + vPID->pre2Error);
	vPID->pre2Error = vPID->preError;
	vPID->preError = err;
	vPID->result = vPID->result + increment;

    /* 输出限幅 */
    if(vPID->result > vPID->maximum)
        vPID->result = vPID->maximum;
    else if(vPID->result < vPID->minimum)
        vPID->result = vPID->minimum;
}


//函数名称：专家型PID实现
void ExpertPidRealize(PID* vPID)
{
	float thiserror;            //本次误差
	float deltaerror;           //本次误差偏差
	float lastdeltaerror;       //上一次误差偏差
	float result;               //本次调节输出值

    thiserror = vPID->setPoint - vPID->actualPoint; //计算本次误差
    deltaerror = thiserror - vPID->preError;        //计算本次误差偏差
    lastdeltaerror = vPID->preError - vPID->pre2Error; //计算上次误差偏差

    //（1）判断本次误差是否查过误差极限最大值
	if(myfabs(thiserror) >= vPID->errorabsmax)
	{    /*执行规则1  偏差最大情况下全负荷输出*/
		if(thiserror > 0)
		{
            result = vPID->maximum;
        }
		if(thiserror < 0)
		{
            result = vPID->minimum;
        }
	}

	if((thiserror*deltaerror>0)||(deltaerror==0))
	{/*执行规则2    误差与其导数同向情况下*/
		if(myfabs(thiserror)>=vPID->errorabsmid) /* 误差范围 (errorabsmid,errorabsmax)*/
		{
			result=vPID->result+2.0*(vPID->KP*deltaerror+vPID->KI*thiserror+vPID->KD*(deltaerror-lastdeltaerror));
		}
		else
		{
			result=vPID->result+0.4*(vPID->KP*deltaerror+vPID->KI*thiserror+vPID->KD*(deltaerror-lastdeltaerror));
		}
	}

	if(((thiserror*deltaerror<0)&&(deltaerror*lastdeltaerror>0))||(thiserror==0))
	{/*执行规则3    误差与其导数反向情况下且刚刚反向*/
		result = vPID->result;
	}

	if((thiserror*deltaerror<0)&&(deltaerror*lastdeltaerror<0))
	{/*执行规则4    误差处于极值状态*/
		if(myfabs(thiserror)>=vPID->errorabsmid)/* 误差范围 (errorabsmid,errorabsmax)*/
		{
			result=vPID->result+2.0*vPID->KP*thiserror;
		}
		else    
		{
			result=vPID->result+0.6*vPID->KP*thiserror;
		}
	}

	if((myfabs(thiserror)<=vPID->errorabsmin)&&(myfabs(thiserror)>0))
	{   /*执行规则5 误差小于下界*         误差范围 (0,errorabsmin)*/
		result=vPID->result+0.5*vPID->KP*deltaerror+0.3*vPID->KI*thiserror;
	}

	/*输出限值避免超调*/
	if(result >= vPID->maximum)
	{
        result = vPID->maximum;
    }
	if(result <= vPID->minimum)
	{
        result = vPID->minimum;
    }

	vPID->result=result;
	vPID->pre2Error=vPID->preError;
	vPID->preError=thiserror;
}